From 330a39ba2cf3934337f6c919a7411e2b1111068a Mon Sep 17 00:00:00 2001 From: Robert Lipe Date: Tue, 16 May 2017 23:58:47 -0500 Subject: [PATCH] Make GPGSA more robust for short sentences, more C++-like. Add warning for badk checksums. --- nmea.cc | 100 +++++++++++++++++++++++++++----------------------------- 1 file changed, 49 insertions(+), 51 deletions(-) diff --git a/nmea.cc b/nmea.cc index 150f68519..e259d2f57 100644 --- a/nmea.cc +++ b/nmea.cc @@ -26,6 +26,7 @@ #include "gbser.h" #include "strptime.h" #include "jeeps/gpsmath.h" +#include "src/core/logging.h" #include #include @@ -69,12 +70,12 @@ ' $GPRMC - Recommended minimum specific GNSS Data ' $GPRMC,085721.194,A,5917.7210,N,01103.9227,E,21.42,50.33,300504,,*07 ' 2 085721 Fix taken at 08:57:21 UTC - ' 3 A Fix valid (this field reads V if fix is not valid) + ' 3 A Fix valid (this field reads V if fix is not valid) ' 4,5 5917.7210,N Latitude 59 deg 17.7210' N ' 6,7 01103.9227,E Longitude 11 deg 03.9227' E - ' 8 21.42 Speed over ground (knots) - ' 9 50.33 Course over ground (true) - ' 10 300504 Date 30/05-2004 + ' 8 21.42 Speed over ground (knots) + ' 9 50.33 Course over ground (true) + ' 10 300504 Date 30/05-2004 ' 11 Empty field Magnetic variation GSA - GPS DOP and active satellites @@ -505,9 +506,9 @@ gpgga_parse(char* ibuf) WAYPT_SET(waypt, geoidheight, geoidheight); - waypt->sat = nsats; + waypt->sat = nsats; - waypt->hdop = hdop; + waypt->hdop = hdop; switch (fix) { case 0: @@ -678,49 +679,48 @@ gpzda_parse(char* ibuf) // This can't have worked. } +// This function has had a hard life. It began as a moderately terrifying +// bunch of nested sscanfs. In 2017, robertl replaced the scanf stuff with +// a QString::split() to make it more tolerant of really empty fields from +// certain GPS implementations, but didn't replace the (somewhat funky) back +// half to match the parse. There are definitely some readibility issues +// here. +// The numbering as per http://aprs.gids.nl/nmea/#gsa was the reference as +// the field numbers conveniently match our index. static void gpgsa_parse(char* ibuf) { - char fixauto; - char fix; int prn[12] = {0}; - int scn,cnt; - float pdop=0,hdop=0,vdop=0; - char* tok=0; - memset(prn,0xff,sizeof(prn)); - scn = sscanf(ibuf,"$%*2cGSA,%c,%c,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d", - &fixauto, &fix, - &prn[0],&prn[1],&prn[2],&prn[3],&prn[4],&prn[5], - &prn[6],&prn[7],&prn[8],&prn[9],&prn[10],&prn[11]); - if (scn < 2) { - warning(MYNAME ": Short GSA sentence.\n"); - } - /* - sscanf has scanned all the leftmost elements - we'll rescan by skipping 15 commas to the dops - */ - tok = ibuf; - for (cnt=0; (tok)&&(cnt<15); cnt++) { - tok = strchr(tok,','); - if (!tok) { - break; - } - tok++; + QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts); + int nfields = fields.size(); + // 0 = "GPGSA" + // 1 = Mode. Ignored + QChar fix; + if (nfields > 1) { + fix = fields[2][0]; } - if (tok) { - sscanf(tok,"%f,%f,%f",&pdop,&hdop,&vdop); + // 12 fields, index 3 through 14. + for (int cnt = 0; cnt <= 11; cnt++) { + if (nfields >= cnt + 2) prn[cnt] = fields[cnt + 3].toInt(); } + float pdop = 0, hdop = 0, vdop = 0; + if (nfields > 14) pdop = fields[15].toDouble(); + if (nfields > 15) hdop = fields[16].toDouble(); + if (nfields > 16) { + // Last one is special. The checksum isn't split out above. + fields[17].chop(3); + vdop = fields[17].toDouble(); + } if (curr_waypt) { - if (curr_waypt->fix!=fix_dgps) { - if (fix=='3') { - curr_waypt->fix=fix_3d; - } else if (fix=='2') { - curr_waypt->fix=fix_2d; + if (fix=='3') { + curr_waypt->fix = fix_3d; + } else if (fix=='2') { + curr_waypt->fix = fix_2d; } } @@ -728,9 +728,9 @@ gpgsa_parse(char* ibuf) curr_waypt->hdop = hdop; curr_waypt->vdop = vdop; - if (curr_waypt->sat <= 0) { - for (cnt=0; cnt<12; cnt++) { - curr_waypt->sat += (prn[cnt]>0)?(1):(0); + if (curr_waypt->sat <= 0) { + for (int cnt = 0; cnt <= 11; cnt++) { + curr_waypt->sat += (prn[cnt] > 0) ? 1 : 0; } } } @@ -838,7 +838,7 @@ pcmpt_parse(char* ibuf) /* * Since we oh-so-cleverly inserted points at the head, * we can rip through the queue forward now to get our - ` * handy-dandy reversing effect. + * handy-dandy reversing effect. */ trk_head = route_head_alloc(); track_add_head(trk_head); @@ -873,7 +873,8 @@ nmea_fix_timestamps(route_head* track) Waypoint* wpt = (Waypoint*)elem; wpt->creation_time += delta_tm; - if ((prev != NULL) && (prev->creation_time > wpt->creation_time)) { /* go over midnight ? */ + if ((prev != NULL) && (prev->creation_time > wpt->creation_time)) { + /* go over midnight ? */ delta_tm += SECONDS_PER_DAY; wpt->creation_time += SECONDS_PER_DAY; } @@ -883,7 +884,7 @@ nmea_fix_timestamps(route_head* track) time_t prev; queue* elem; - tm.tm_hour = 23; /* last date found */ + tm.tm_hour = 23; /* last date found */ tm.tm_min = 59; tm.tm_sec = 59; @@ -897,7 +898,7 @@ nmea_fix_timestamps(route_head* track) if (wpt->wpt_flags.fmt_use != 0) { time_t dt; - wpt->wpt_flags.fmt_use = 0; /* reset flag */ + wpt->wpt_flags.fmt_use = 0; /* reset flag */ dt = (prev / SECONDS_PER_DAY) * SECONDS_PER_DAY; wpt->creation_time += dt; @@ -951,10 +952,7 @@ nmea_parse_one_line(char* ibuf) ck++; sscanf(ck, "%2X", &ckcmp); if (ckval != ckcmp) { -#if 0 - printf("ckval %X, %X, %s\n", ckval, ckcmp, ck); - printf("NMEA %s\n", tbuf); -#endif + Warning() << "Invalid NMEA checksum. Computed " << ckval << " but found " << ckcmp << ". Ignoring sentence"; return; } @@ -1326,7 +1324,7 @@ nmea_trackpt_pr(const Waypoint* wpt) /* GISTeq doesn't care about the checksum, but wants this prefixed, so * we can write it with abandon. - */ + */ if (opt_gisteq) { gbfprintf(file_out, "---,"); } @@ -1402,7 +1400,7 @@ nmea_wr_posn(Waypoint* wpt) static void nmea_wr_posn_deinit(void) { -// nmea_wr_deinit(); +// nmea_wr_deinit(); } @@ -1421,7 +1419,7 @@ ff_vecs_t nmea_vecs = { nmea_write, NULL, nmea_args, - CET_CHARSET_ASCII, 0, /* CET-REVIEW */ + CET_CHARSET_ASCII, 0, /* CET-REVIEW */ { nmea_rd_posn_init, nmea_rd_posn, nmea_rd_deinit, nmea_wr_posn_init, nmea_wr_posn, nmea_wr_posn_deinit -- 2.30.2